#include "pathfinding.h"
Graph::Graph(int width, int height, vector<Object *> *objs){
    build(width, height);
}

void Graph::build(int width, int height){
    for(int i = 0; i < width; i += 16){
        for(int j = 0; j < height; j += 16){
            nodes.push_back(new Node(i, j));
            cout << "added Node [" << i << ", " << j << "]" << endl;
        }
    }
    //vector<Entity*> objects;

    //objects.push_back(new Object(192, 224, 320, 144));
    //setObsticals(objects);
    connectNodes();
    cout << "graph finished" << endl;
}
void Graph::connectNodes(){
    for(Node *n : nodes){
        n->setNeighbors(nodes);
    }
}

void Node::setNeighbors(vector<Node*> &nodes){
    //cout << "in GetNeighbors" << endl;
    for(Node *n : nodes){
        if(neighbors.size() == 4){
            break;
        }
        //cout << "evaluating node [" << x << ", " << y <<"]" << endl;
        if(n->x == x && n->y == y + 16){
            neighbors.push_back(n);
            //cout << "connected node [" << x << ", " << y << "] to node [" << n->x << ", " << n->y << "]" << endl;
        } else if(n->x == x && n->y == y - 16){
            neighbors.push_back(n);
            //cout << "connected node [" << x << ", " << y << "] to node [" << n->x << ", " << n->y << "]" << endl;
        } else if(n->x == x + 16 && n->y == y){
            neighbors.push_back(n);
            //cout << "connected node [" << x << ", " << y << "] to node [" << n->x << ", " << n->y << "]" << endl;
        } else if(n->x == x - 16 && n->y == y){
            neighbors.push_back(n);
            //cout << "connected node [" << x << ", " << y << "] to node [" << n->x << ", " << n->y << "]" << endl;
        }
    }
}
void Graph::setObsticals(vector<Entity*> objs){
    for(Entity * obj : objs){
        for(Node *n : nodes){
            if(n->getX() >= obj->getX() && n->getX() <= (obj->getX() + obj->getWidth() + 16) && n->getY() >= obj->getY() -16 && n->getY() <= obj->getY() + obj->getHeight()){
                n->setPassable(false);
            }
        }
    }
}
Node* Graph::getNode(int x, int y){
    for(Node *n : nodes){
        cout << "geting" << endl;
        if(n->getX() == x && n->getY() == y){
            cout << "got" << endl;
            return n;
        }
    }
    return nullptr;
}

vector<Node*> *Graph::Getpath(int targetX, int targetY){
    queue<Node*> frontier;
    unordered_map<Node*, Node*> came_from;
    Node *goal = getNode(targetX, targetY);
    Node *start = getNode(112, 112);
    frontier.push(start);
    came_from[start] = start;
    cout << "start: [" << start->getX() << ", " << start->getY() << "]" << endl;
    cout << "goal: [" << goal->getX() << ", " << goal->getY() << "]" << endl;

    while(frontier.size() != 0){
        Node* current = frontier.front();
        frontier.pop();
        if(current->getX() == goal->getX() && current->getY() == goal->getY()){
        //if(current == goal){
            break;
        }

        for(Node* next : current->getNeighbors()){
            if((!came_from.count(next))){
                if(next->isPassable()){
                    frontier.push(next);
                    came_from[next] = current;
                }
                else {
                    cout << "not passable" << endl;
                }
            }
        }

    }
    vector<Node*> *path = new vector<Node  *>;
    Node *current;
    current = goal;
    path->push_back(current);
    cout << current->getX() << ", " << current->getY() << endl;
    //while(current->getX() != start->getX() && current->getY() != start->getY()){
    while(current != start) {
        current = came_from[current];
        if(current != nullptr){
            cout << current->getX() << ", " << current->getY() << endl;
            path->push_back(current);
        }
    }
    return path;



}
